#include "stdafx.h"
#include "Rtu.h"
#include <sstream>
#include <boost/format.hpp>     
#include <boost/tokenizer.hpp>     
#include <boost/algorithm/string.hpp>   
#include <vector>
#include <string>
#include <boost/algorithm/string.hpp>

CRtu::CRtu(void):_ctx(NULL)
{
}


CRtu::~CRtu(void)
{
}


bool CRtu::Open(const std::string& serial, int slave)
{
	uint32_t old_response_to_sec;
    uint32_t old_response_to_usec;
    uint32_t new_response_to_sec;
    uint32_t new_response_to_usec;

	_ctx = modbus_new_rtu(serial.c_str(), 9600, 'N', 8, 1);

	if (_ctx == NULL) {
        fprintf(stderr, "Unable to allocate libmodbus context\n");
        return false;
    }

    modbus_set_debug(_ctx, FALSE);
    modbus_set_error_recovery(_ctx, MODBUS_ERROR_RECOVERY_ALL);
    modbus_set_slave(_ctx, slave);


    modbus_get_response_timeout(_ctx, &old_response_to_sec, &old_response_to_usec);
    if (modbus_connect(_ctx) == -1) {
        fprintf(stderr, "Connection failed: %s\n", modbus_strerror(errno));
        modbus_free(_ctx);
        return false;
    }
    modbus_get_response_timeout(_ctx, &new_response_to_sec, &new_response_to_usec);

	return true;
}

std::string CRtu::ReadBits(int adress, int len){
	if(_ctx == NULL)
		return "error";
	std::stringstream ss;
	uint8_t * tab_rp_bits = (uint8_t *) malloc(len * sizeof(uint8_t));
    memset(tab_rp_bits, 0, len * sizeof(uint8_t));
	int rc = modbus_read_bits(_ctx, adress, len, tab_rp_bits);
	if(rc != len)
		return "error";
	for(int i=0; i< len; ++i)
		ss << tab_rp_bits[i];
	free(tab_rp_bits);
	return ss.str();
}

std::string CRtu::ReadRegistors(int adress, int len){
	if(_ctx == NULL)
		return "error";
	std::stringstream ss;
	uint16_t * tab_rp_registers = (uint16_t *) malloc(len * sizeof(uint16_t));
    memset(tab_rp_registers, 0, len * sizeof(uint16_t));
	int rc = modbus_read_registers(_ctx, adress,
                               len, tab_rp_registers);
	if(rc != len)
		return "error";
	for(int i=0; i<len; ++i)
		ss << tab_rp_registers[i]<< "-";
	free(tab_rp_registers);
	return ss.str();
}


bool CRtu::Close()
{
	if(_ctx != NULL){
		modbus_close(_ctx);
		modbus_free(_ctx);
		_ctx = NULL;
	}
	
	return true;
}

std::string CRtu::WriteBits(int adress, unsigned int len, std::string& req){
	if(_ctx == NULL)
		return "error";

	try{
		std::vector<std::string> vecSegTag;  
		boost::split(vecSegTag, req, boost::is_any_of(_T("-&")));    
		uint8_t * tab_rp_bits = (uint8_t *) malloc(len * sizeof(uint8_t)); 
		if(vecSegTag.size()/2 < len){
			
			memset(tab_rp_bits, 0, len * sizeof(uint8_t));
			int rc = modbus_read_bits(_ctx, adress, len, tab_rp_bits);
			if(rc != len)
				return "error";
		}

		for (size_t i = 0; i<vecSegTag.size()/2; i++)  
		{    
			int nIndex = atoi(vecSegTag[i*2].c_str()); 
			if(nIndex < len)
				tab_rp_bits[nIndex] = atoi(vecSegTag[i*2+1].c_str()); 
		}
		int rc = modbus_write_bits(_ctx, adress, len, tab_rp_bits);
		
		free(tab_rp_bits);
		if(rc != len)
			return "error";

	}catch(...)
	{
		return "error";
	}
	
	return "ok";
}